/**
 * @file map.cpp
 * @author BadFatCat0919 (543015378@qq.com)
 * @brief 地图类
 */

#include "Map.h"
#include "log.h"
#include <cmath>
#include <cstring>
#include "Params.h"

/**
 * @brief 移动一步
 * @param dir 移动方向
 */
void MapNode::moveStep(int dir) {
    switch(dir) {
        case Direction::RIGHT:
            this->x += 1;
            break;
        case Direction::UP_RIGHT:
            this->y += 1;
            this->x += 1;
            break;
        case Direction::UP:
            this->y += 1;
            break;
        case Direction::UP_LEFT:
            this->y += 1;
            this->x -= 1;
            break;
        case Direction::LEFT:
            this->x -= 1;
            break;
        case Direction::DOWN_LEFT:
            this->y -= 1;
            this->x -= 1;
            break;
        case Direction::DOWN:
            this->y -= 1;
            break;
        case Direction::DOWN_RIGHT:
            this->y -= 1;
            this->x += 1;
            break;
        default:
            break;
    }
}

/**
 * @brief 创建地图结点
 * @param x 横坐标
 * @param y 纵坐标
 * @param dir 移动方向
 */
MapNode::MapNode(int x, int y, int dir) : x(x), y(y) {
    this->moveStep(dir);
}

Map::benchLocate Map::benchs[BENCH_NUM_MAX];
Map::robotLocate Map::robots[ROBOT_NUM_MAX];
char Map::map[MAP_X_MAX][MAP_Y_MAX];
char Map::mapFull[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];
char Map::mapEmpty[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];
char Map::mapObstacleFull[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];
char Map::mapObstacleEmpty[MAP_X_MAX_EXPAND][MAP_Y_MAX_EXPAND];
int Map::id = 0;

/**
 * @brief 判断点是否在地图内
*/
bool Map::isInMap(int x, int y) {
    return !(x < 0 || x >= MAP_X_MAX_EXPAND || y < 0 || y >= MAP_Y_MAX_EXPAND);
};

/**
 * @brief 录入地图点
 * @param x 横坐标
 * @param y 纵坐标
 * @param tag 区域类型
 */
void Map::setPoint(int x, int y, char tag) {
    if(!Map::isInMap(x, y)) {
        return;
    }
    Map::map[x][y] = tag;
    for(int i = 3*x; i < 3*(x+1); ++i) {
        for(int j = 3*y; j < 3*(y+1); ++j) {
            if(tag == MAP_TAG_OBSTACLE) {
                Map::mapEmpty[i][j] = Map::mapFull[i][j] = MAP_TAG_OBSTACLE;
            } else {
                Map::mapEmpty[i][j] = Map::mapFull[i][j] = MAP_TAG_GROUND;
            }
        }
    }
}

/**
 * @brief 膨胀障碍物
 */
void Map::expandObstacle() {
    // 空载地图，边界内收两格，障碍物八个方向扩两格
    for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
        for(int y = 0; y < 2; ++y) {
            if(Map::mapEmpty[x][y] == MAP_TAG_GROUND) {
                Map::mapEmpty[x][y] = MAP_TAG_PADDING;
            }
        }
        for(int y = MAP_Y_MAX_EXPAND - 2; y < MAP_Y_MAX_EXPAND; ++y) {
            if(Map::mapEmpty[x][y] == MAP_TAG_GROUND) {
                Map::mapEmpty[x][y] = MAP_TAG_PADDING;
            }
        }
    }
    for(int y = 2; y < MAP_Y_MAX_EXPAND - 2; ++y) {
        for(int x = 0; x < 2; ++x) {
            if(Map::mapEmpty[x][y] == MAP_TAG_GROUND) {
                Map::mapEmpty[x][y] = MAP_TAG_PADDING;
            }
        }
        for(int x = MAP_X_MAX_EXPAND - 2; x < MAP_X_MAX_EXPAND; ++x) {
            if(Map::mapEmpty[x][y] == MAP_TAG_GROUND) {
                Map::mapEmpty[x][y] = MAP_TAG_PADDING;
            }
        }
    }
    for(int x = 2; x < MAP_X_MAX_EXPAND - 2; ++x) {
        for(int y = 2; y < MAP_Y_MAX_EXPAND - 2; ++y) {
            if(Map::mapEmpty[x][y] == MAP_TAG_OBSTACLE) {
                for(int i = x - 2; i <= x + 2; ++i) {
                    for(int j = y - 2; j <= y + 2; ++j) {
                        if(Map::mapEmpty[i][j] == MAP_TAG_GROUND) {
                            Map::mapEmpty[i][j] = MAP_TAG_PADDING;
                        }
                    }
                }
            }
        }
    }

    // 满载地图，边界内收三格，障碍物八个方向扩两格，上下左右方向多扩一格，所有一圈只有两个可走点的格子也堵死
    for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
        for(int y = 0; y < 3; ++y) {
            if(Map::mapFull[x][y] == MAP_TAG_GROUND) {
                Map::mapFull[x][y] = MAP_TAG_PADDING;
            }
        }
        for(int y = MAP_Y_MAX_EXPAND - 3; y < MAP_Y_MAX_EXPAND; ++y) {
            if(Map::mapFull[x][y] == MAP_TAG_GROUND) {
                Map::mapFull[x][y] = MAP_TAG_PADDING;
            }
        }
    }
    for(int y = 3; y < MAP_Y_MAX_EXPAND - 3; ++y) {
        for(int x = 0; x < 3; ++x) {
            if(Map::mapFull[x][y] == MAP_TAG_GROUND) {
                Map::mapFull[x][y] = MAP_TAG_PADDING;
            }
        }
        for(int x = MAP_X_MAX_EXPAND - 3; x < MAP_X_MAX_EXPAND; ++x) {
            if(Map::mapFull[x][y] == MAP_TAG_GROUND) {
                Map::mapFull[x][y] = MAP_TAG_PADDING;
            }
        }
    }
    for(int x = 3; x < MAP_X_MAX_EXPAND - 3; ++x) {
        for(int y = 3; y < MAP_Y_MAX_EXPAND - 3; ++y) {
            if(Map::mapFull[x][y] == MAP_TAG_OBSTACLE) {
                for(int i = x - 2; i <= x + 2; ++i) {
                    for(int j = y - 2; j <= y + 2; ++j) {
                        if(Map::mapFull[i][j] == MAP_TAG_GROUND) {
                            Map::mapFull[i][j] = MAP_TAG_PADDING;
                        }
                    }
                }
                if(Map::isInMap(x - 3, y)) {
                    if(Map::mapFull[x - 3][y] == MAP_TAG_GROUND) {
                        Map::mapFull[x - 3][y] = MAP_TAG_PADDING;
                    }
                }
                if(Map::isInMap(x + 3, y)) {
                    if(Map::mapFull[x + 3][y] == MAP_TAG_GROUND) {
                        Map::mapFull[x + 3][y] = MAP_TAG_PADDING;
                    }
                }
                if(Map::isInMap(x, y - 3)) {
                    if(Map::mapFull[x][y - 3] == MAP_TAG_GROUND) {
                        Map::mapFull[x][y - 3] = MAP_TAG_PADDING;
                    }
                }
                if(Map::isInMap(x, y + 3)) {
                    if(Map::mapFull[x][y + 3] == MAP_TAG_GROUND) {
                        Map::mapFull[x][y + 3] = MAP_TAG_PADDING;
                    }
                }
            }
        }
    }
    for(int x = 3; x < MAP_X_MAX_EXPAND - 3; ++x) {
        for(int y = 3; y < MAP_Y_MAX_EXPAND - 3; ++y) {
            if(Map::mapFull[x][y] == MAP_TAG_GROUND) {
                int ground = 0;
                for(int i = x - 1; ground <= 3 && i <= x + 1; ++i) {
                    for(int j = y - 1; ground <= 3 && j <= y + 1; ++j) {
                        ground += (Map::mapFull[i][j] == MAP_TAG_GROUND);
                    }
                }
                if(ground <= 3) {
                    Map::mapFull[x][y] = MAP_TAG_PADDING;
                }
            }
        }
    }
}

/**
 * @brief 将地图打印到LOG中
 */
void Map::printToLog() {
    for(int y = MAP_Y_MAX_EXPAND-1; y >= 0; --y) {
        LOG("%03d ", y);
        for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
            char out = MAP_TAG_GROUND;
            switch(Map::mapEmpty[x][y]) {
                case MAP_TAG_PADDING:
                    out = MAP_TAG_PADDING;
                    break;
                case MAP_TAG_OBSTACLE:
                    out = MAP_TAG_OBSTACLE;
                    break;
                default:
                    out = Map::mapEmpty[x][y];
                    break;
            }
            LOG("%3c ", out);
        }
        LOG("\n");
    }
    LOG("     ");
    for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
        LOG("%03d ", x);
    }
    LOG("\n");

    for(int y = MAP_Y_MAX_EXPAND-1; y >= 0; --y) {
        LOG("%03d ", y);
        for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
            char out = MAP_TAG_GROUND;
            switch(Map::mapFull[x][y]) {
                case MAP_TAG_PADDING:
                    out = MAP_TAG_PADDING;
                    break;
                case MAP_TAG_OBSTACLE:
                    out = MAP_TAG_OBSTACLE;
                    break;
                default:
                    out = Map::mapFull[x][y];
                    break;
            }
            LOG("%3c ", out);
        }
        LOG("\n");
    }
    LOG("     ");
    for(int x = 0; x < MAP_X_MAX_EXPAND; ++x) {
        LOG("%03d ", x);
    }
    LOG("\n");
}

/**
 * @brief 地图初始化
 */
void Map::init() {
    Map::expandObstacle();
}

/**
 * @brief 设置地图ID
 * @param id 地图ID
 */
void Map::setID(int id) {
    Map::id = id;
}

/**
 * @brief 获得地图ID
 * @return int 地图ID
 */
int Map::getID() {
    return Map::id;
}

/**
 * @brief 判断某点是否有障碍
 * @param x 横坐标
 * @param y 纵坐标
 * @param isEmpty 是否使用空载状态的地图
 * @return true 有障碍
 * @return false 无障碍
 */
bool Map::isObstacle(int x, int y, bool isEmpty) {
    auto useMap = isEmpty ? Map::mapEmpty : Map::mapFull;
    return useMap[x][y] == MAP_TAG_OBSTACLE || useMap[x][y] == MAP_TAG_PADDING;
}

/**
 * @brief 判断某点是否有机器人
 * @param x 横坐标
 * @param y 纵坐标
 * @param exceptID 排除的机器人序号
 * @param isEmpty 是否使用空载状态的地图
 * @param dir 机器人的方向
 * @return int 机器人序号(-1表示没有机器人)
 */
int Map::isRobot(int x, int y, int exceptID, bool isEmpty, int dir) {
    for(int id = 0; id < ROBOT_NUM_MAX; ++id) {
        if(id != exceptID) {
            int warnDistance = (isEmpty ? PARAMS.SAFETY_DISTANCE_EMPTY : PARAMS.SAFETY_DISTANCE_FULL) + 
                               (Map::robots[id].isEmpty ? PARAMS.SAFETY_DISTANCE_EMPTY : PARAMS.SAFETY_DISTANCE_FULL);
            if(std::abs(Map::robots[id].x - x) <= warnDistance && std::abs(Map::robots[id].y - y) <= warnDistance) {
                if(dir != Direction::INVALID) {
                    if(Direction::GetDirectionDiff(dir, Map::robots[id].dir) <= 2) {
                        continue;
                    }
                }
                return id;
            }
        }
    }
    return -1;
}

/**
 * @brief 判断某点是否有工作台
 * @param x 横坐标
 * @param y 纵坐标
 * @return int 工作台序号(-1表示没有工作台)
 */
int Map::isBench(int x, int y) {
    x /= 3, y /= 3;
    for(int id = 0; id < Bench::getTotalNumber(); ++id) {
        if(x == Map::benchs[id].x/3 && y == Map::benchs[id].y/3) {
            return id;
        }
    }
    return -1;
}

/**
 * @brief 刷新工作台位置
 * @param benchs 工作台数组
 */
void Map::refreshBenchLocation(Bench benchs[]) {
    for(int id = 0; id < Bench::getTotalNumber(); ++id) {
        Map::benchs[id].x = benchs[id].getCellX();
        Map::benchs[id].y = benchs[id].getCellY();
        Map::benchs[id].type = benchs[id].getType();
    }
}

/**
 * @brief 刷新机器人位置
 * @param dispatchers 调度器数组
 */
void Map::refreshRobotLocation(Dispatcher dispatchers[]) {
    for(int id = 0; id < ROBOT_NUM_MAX; ++id) {
        Map::robots[id].x = dispatchers[id].getRobot()->getCellX();
        Map::robots[id].y = dispatchers[id].getRobot()->getCellY();
        Map::robots[id].isEmpty = dispatchers[id].getRobot()->isEmpty();
        if(dispatchers[id].isFree() || dispatchers[id].getCmd().type != Cmd::RUN_TO) {
            Map::robots[id].dir = Direction::INVALID;
        } else {
            Map::robots[id].dir = dispatchers[id].getCmd().data3;
        }
    }
}

/**
 * @brief 获取工作台位置
 * @param id 工作台ID
 * @return robotLocate* 工作台位置结构体
 */
Map::benchLocate *Map::getBenchLocate(int id) {
    return &Map::benchs[id];
}

/**
 * @brief 获取机器人当前位置
 * @param id 机器人ID
 * @return robotLocate* 机器人位置结构体
 */
Map::robotLocate *Map::getRobotLocate(int id) {
    return &Map::robots[id];
}
